﻿/********************************************************
 *  ██████╗  ██████╗████████╗██╗
 * ██╔════╝ ██╔════╝╚══██╔══╝██║
 * ██║  ███╗██║        ██║   ██║
 * ██║   ██║██║        ██║   ██║
 * ╚██████╔╝╚██████╗   ██║   ███████╗
 *  ╚═════╝  ╚═════╝   ╚═╝   ╚══════╝
 * Geophysical Computational Tools & Library (GCTL)
 *
 * Copyright (c) 2023  Yi Zhang (yizhang-geo@zju.edu.cn)
 *
 * GCTL is distributed under a dual licensing scheme. You can redistribute 
 * it and/or modify it under the terms of the GNU Lesser General Public 
 * License as published by the Free Software Foundation, either version 2 
 * of the License, or (at your option) any later version. You should have 
 * received a copy of the GNU Lesser General Public License along with this 
 * program. If not, see <http://www.gnu.org/licenses/>.
 * 
 * If the terms and conditions of the LGPL v.2. would prevent you from using 
 * the GCTL, please consider the option to obtain a commercial license for a 
 * fee. These licenses are offered by the GCTL's original author. As a rule, 
 * licenses are provided "as-is", unlimited in time for a one time fee. Please 
 * send corresponding requests to: yizhang-geo@zju.edu.cn. Please do not forget 
 * to include some description of your company and the realm of its activities. 
 * Also add information on how to contact you by electronic and paper mail.
 ******************************************************/

#include "space_filter.h"

// 二维平面上的椭圆型滤波器
double gctl::ellipse_filter(double in_x, double in_y, const ellipse_filter_para &elf, 
    filter_base_e base_func)
{
    double filter;
    double xd = in_x - elf.mu_x;
    double yd = in_y - elf.mu_y;

    double xy_module = sqrt(pow(xd,2) + pow(yd,2) + GCTL_ZERO);
    double t = GCTL_SIGN(yd)*acos(xd/xy_module);
    double weight = sqrt(pow(elf.len_x*cos(t - elf.theta*GCTL_Pi/180.0), 2) + 
        pow(elf.len_y*sin(t - elf.theta*GCTL_Pi/180.0), 2));

    if(base_func == Atan) //使用arctan构造加权函数
    {
        filter = GCTL_Pi - 2.0*atan(elf.rho*weight*xy_module - elf.rho*elf.sigma);
        filter = filter/(GCTL_Pi + 2.0*atan(elf.rho*elf.sigma));
        return filter;
    }

    //使用以e为底的幂函数构造加权函数
    return elf.magnify*pow(2.0, -1.0*weight*weight*xy_module*xy_module/(elf.sigma*elf.sigma));
}

// 一维高斯滤波器
void gctl::gauss1d_filter(const array<double> &in_data, array<double> &out_data, double sigma, int k_size)
{
    if (sigma <= 0.0)
    {
        throw std::runtime_error("Invalid sigma value (must be >= 0). From gctl::gauss1d_filter(...)");
    }

    if (k_size < 3 || k_size/2 == 0)
    {
        throw std::runtime_error("Invalid kernel size (must be >= 3 and odd). From gctl::gauss1d_filter(...)");
    }
    
    // Prepare the gauss kernel
    array<double> gkernel(k_size, 0.0);

    int half_k = k_size/2;
    double x;
    for (int i = 0; i < k_size; i++)
    {
        x = i - half_k;
        gkernel[i] = exp(x*x/(-2.0*sigma*sigma))/(sqrt(2.0*GCTL_Pi)*sigma);
    }
    // End preparing the gauss kernel

    // Copy the input data to output ones
    out_data = in_data;

    double d_sum, c_sum;
    int id, d_size = in_data.size();
    for (int i = 0; i < d_size; i++)
    {
            d_sum = c_sum = 0.0;
            for (int p = 0; p < k_size; p++)
            {
                    id = i + p - half_k;
                    if (id >= 0 && id < d_size)
                    {
                        d_sum += gkernel[p]*in_data[id];
                        c_sum += gkernel[p];
                    }
            }

            out_data[i] = d_sum/c_sum;
    }
    return;
}

// 二维高斯滤波器
void gctl::gauss2d_filter(const array<double> &in_data, array<double> &out_data, int a1_num, int a2_num, double sigma, int k_size)
{
    if (sigma <= 0.0)
    {
        throw std::runtime_error("Invalid sigma value (must be >= 0). From gctl::gauss2d_filter(...)");
    }

    if (k_size < 3 || k_size/2 == 0)
    {
        throw std::runtime_error("Invalid kernel size (must be >= 3 and odd). From gctl::gauss2d_filter(...)");
    }
    
    // Prepare the gauss kernel
    matrix<double> gkernel(k_size, k_size, 0.0);

    int half_k = k_size/2;
    double x, y;
    for (int i = 0; i < k_size; i++)
    {
        for (int j = 0; j < k_size; j++)
        {
            y = i - half_k;
            x = j - half_k;

            gkernel[i][j] = exp((x*x + y*y)/(-2.0*sigma*sigma))/(2.0*GCTL_Pi*sigma*sigma);
        }
    }
    // End preparing the gauss kernel

    // Copy the input data to output ones
    out_data = in_data;

    double d_sum, c_sum;
    int id, id2, xid, yid;
    for (int i = 0; i < a2_num; i++)
    {
        for (int j = 0; j < a1_num; j++)
        {
            id = j + i*a1_num;

            d_sum = c_sum = 0.0;
            for (int p = 0; p < k_size; p++)
            {
                for (int q = 0; q < k_size; q++)
                {
                    xid = j + q - half_k;
                    yid = i + p - half_k;

                    if (xid >= 0 && xid < a1_num && yid >= 0 && yid < a2_num)
                    {
                        id2 = xid + yid*a1_num;

                        d_sum += gkernel[p][q]*in_data[id2];
                        c_sum += gkernel[p][q];
                    }
                }
            }

            out_data[id] = d_sum/c_sum;
        }
    }
    return;
}

// 三维高斯滤波器
void gctl::gauss3d_filter(const array<double> &in_data, array<double> &out_data, int a1_num, int a2_num, int a3_num, double sigma, int k_size)
{
    if (sigma <= 0.0)
    {
        throw std::runtime_error("Invalid sigma value (must be >= 0). From gctl::gauss3d_filter(...)");
    }

    if (k_size < 3 || k_size/2 == 0)
    {
        throw std::runtime_error("Invalid kernel size (must be >= 3 and odd). From gctl::gauss3d_filter(...)");
    }
    
    // Prepare the gauss kernel
    array<double> gkernel(k_size*k_size*k_size, 0.0);

    int id, half_k = k_size/2;
    double x, y, z;
    for (int i = 0; i < k_size; i++)
    {
        for (int j = 0; j < k_size; j++)
        {
            for (int k = 0; k < k_size; k++)
            {
                z = 1.0*(i - half_k);
                y = 1.0*(j - half_k);
                x = 1.0*(k - half_k);

                id = i*k_size*k_size + j*k_size + k;
                gkernel[id] = exp((x*x + y*y + z*z)/(-2.0*sigma*sigma))/(2.0*GCTL_Pi*sigma*sigma);
            }
        }
    }
    // End preparing the gauss kernel

    // Copy the input data to output ones
    out_data = in_data;

    double d_sum, c_sum;
    int id2, xid, yid, zid;
    for (int i = 0; i < a3_num; i++)
    {
        for (int j = 0; j < a2_num; j++)
        {
            for (int k = 0; k < a1_num; k++)
            {
                id = k + j*a1_num + i*a1_num*a2_num;

                d_sum = c_sum = 0.0;
                for (int p = 0; p < k_size; p++)
                {
                    for (int q = 0; q < k_size; q++)
                    {
                        for (int r = 0; r < k_size; r++)
                        {
                            // 在端点处将内部值翻出
                            /*
                            xid = abs(k + r - half_k);
                            yid = abs(j + q - half_k);
                            zid = abs(i + p - half_k);

                            if (xid >= a1_num) xid = 2*(a1_num - 1) - xid;
                            if (yid >= a2_num) yid = 2*(a2_num - 1) - yid;
                            if (zid >= a3_num) zid = 2*(a3_num - 1) - zid;

                            id2 = xid + yid*a1_num + zid*a1_num*a2_num;

                            d_sum += gkernel[p*k_size*k_size + q*k_size + r]*in_data[id2];
                            c_sum += gkernel[p*k_size*k_size + q*k_size + r];
                            */
                            
                            // 在端点处弃用网格之外的值
                            xid = k + r - half_k;
                            yid = j + q - half_k;
                            zid = i + p - half_k;

                            if (xid >= 0 && xid < a1_num && yid >= 0 && yid < a2_num && zid >= 0 && zid < a3_num)
                            {
                                id2 = xid + yid*a1_num + zid*a1_num*a2_num;

                                d_sum += gkernel[p*k_size*k_size + q*k_size + r]*in_data[id2];
                                c_sum += gkernel[p*k_size*k_size + q*k_size + r];
                            }
                        }
                    }
                }

                out_data[id] = d_sum/c_sum;
            }
        }
    }
    return;
}

void gctl::moving_window_1d(int data_len, int win_len, int cur_data_idx, array<int> &win_data_idx)
{
    win_data_idx.resize(win_len);

    int ht = win_len/2;
    for (size_t j = 0; j < win_len; j++)
    {
        win_data_idx[j] = cur_data_idx + j - ht;
        if (cur_data_idx <= ht) win_data_idx[j] = j;
        if (data_len - cur_data_idx <= ht) win_data_idx[j] = data_len  - win_len + j;
    }
    return;
}

void gctl::moving_average_1d(const array<double> &in_data, array<double> &out_data, int win_size)
{
    if (win_size < 3 || win_size/2 == 0)
    {
        throw std::runtime_error("Invalid window size (must be >= 3 and odd). From gctl::moving_average(...)");
    }

    // Copy the input data to output ones
    out_data = in_data;

    double d_sum, c_sum;
    int half_k = win_size/2;
    int id, d_size = in_data.size();
    for (int i = 0; i < d_size; i++)
    {
            d_sum = c_sum = 0.0;
            for (int p = 0; p < win_size; p++)
            {
                    id = i + p - half_k;
                    if (id >= 0 && id < d_size)
                    {
                        d_sum += in_data[id];
                        c_sum += 1.0;
                    }
            }

            out_data[i] = d_sum/c_sum;
    }
    return;
}

void gctl::moving_average_2d(const array<double> &in_data, array<double> &out_data, int a1_num, int a2_num, int win_size)
{
    if (win_size < 3 || win_size/2 == 0)
    {
        throw std::runtime_error("Invalid window size (must be >= 3 and odd). From gctl::moving_average(...)");
    }

    // Copy the input data to output ones
    out_data = in_data;

    double d_sum, c_sum;
    int half_k = win_size/2;
    int id, id2, xid, yid;
    for (int i = 0; i < a2_num; i++)
    {
        for (int j = 0; j < a1_num; j++)
        {
            id = j + i*a1_num;

            d_sum = c_sum = 0.0;
            for (int p = 0; p < win_size; p++)
            {
                for (int q = 0; q < win_size; q++)
                {
                    xid = j + q - half_k;
                    yid = i + p - half_k;

                    if (xid >= 0 && xid < a1_num && yid >= 0 && yid < a2_num)
                    {
                        id2 = xid + yid*a1_num;

                        d_sum += in_data[id2];
                        c_sum += 1.0;
                    }
                }
            }

            out_data[id] = d_sum/c_sum;
        }
    }
    return;
}

void gctl::moving_average_3d(const array<double> &in_data, array<double> &out_data, int a1_num, int a2_num, int a3_num, int win_size)
{
    if (win_size < 3 || win_size/2 == 0)
    {
        throw std::runtime_error("Invalid kernel size (must be >= 3 and odd). From gctl::gauss3d_filter(...)");
    }

    // Copy the input data to output ones
    out_data = in_data;

    double d_sum, c_sum;
    int half_k = win_size/2;
    int id, id2, xid, yid, zid;
    for (int i = 0; i < a3_num; i++)
    {
        for (int j = 0; j < a2_num; j++)
        {
            for (int k = 0; k < a1_num; k++)
            {
                id = k + j*a1_num + i*a1_num*a2_num;

                d_sum = c_sum = 0.0;
                for (int p = 0; p < win_size; p++)
                {
                    for (int q = 0; q < win_size; q++)
                    {
                        for (int r = 0; r < win_size; r++)
                        {   
                            // 在端点处弃用网格之外的值
                            xid = k + r - half_k;
                            yid = j + q - half_k;
                            zid = i + p - half_k;

                            if (xid >= 0 && xid < a1_num && yid >= 0 && yid < a2_num && zid >= 0 && zid < a3_num)
                            {
                                id2 = xid + yid*a1_num + zid*a1_num*a2_num;

                                d_sum += in_data[id2];
                                c_sum += 1.0;
                            }
                        }
                    }
                }

                out_data[id] = d_sum/c_sum;
            }
        }
    }
    return;
}

void gctl::trend_2d(matrix<double> &data, int m_RowNum, int m_ClmNum, int m_OrderNumRow, int m_OrderNumClm)
{
    int i,j,mStep,nStep,i0,j0,k;
	double x0,y0,p;
/////////// To set parameters and sizes of arrays /////////// 
	int SampleNum = m_ClmNum * m_RowNum;
//////// Checking parameters ////////////////
	int MaxOrder, MinOrder;
	if(m_OrderNumRow > m_OrderNumClm)
	{	
		MaxOrder = m_OrderNumRow;
		MinOrder = m_OrderNumClm;
	}
	else
	{
		MaxOrder = m_OrderNumClm;
		MinOrder = m_OrderNumRow;
	}

	int ItemNum = (MaxOrder+1)*(MinOrder+1)-MinOrder*(MinOrder+1)/2;
	if(ItemNum > m_RowNum*m_ClmNum)
	{
		throw std::runtime_error("The polynomial order is too high for this grid data.");
	}

	if((m_OrderNumRow>15)||(m_OrderNumClm>15)||(m_OrderNumRow==0)||(m_OrderNumClm==0))
	{
		throw std::runtime_error("The polynomial order must be within 1 to 15.");
	}

	if(m_RowNum*m_ClmNum<ItemNum)
	{
		throw std::runtime_error("The sampled data point is too few.");
	}
//////// Checking parameters ////////////////
	double **a,**u,**v,*b,*s,*x,*y,*g;
	x=new double [SampleNum]; 
	y=new double [SampleNum]; 
	g=new double [SampleNum]; 	
	b=new double [ItemNum];  
	s=new double [ItemNum];
	a=new double* [ItemNum]; 
	u=new double* [ItemNum]; 
	v=new double* [ItemNum];	
	a[0]=new double [ItemNum*ItemNum]; 
	u[0]=new double [ItemNum*ItemNum]; 
	v[0]=new double [SampleNum*ItemNum];
	for(i=1;i<ItemNum;i++)
	{	
		a[i]=a[i-1]+ItemNum;
		u[i]=u[i-1]+ItemNum;
		v[i]=v[i-1]+ItemNum;
	}

    int m = data.row_size(), n = data.col_size();
///// To obtain x,y and g data for fitting /////////
	mStep=(int)(m/(m_RowNum)); nStep=(int)(n/(m_ClmNum));
	k=0;
	for(i=0;i<m_RowNum;i++)
	{
		i0=i*mStep;
		if((i==(m_RowNum-1))&&((i0>(m-1))||(i0<(m-1))))
			i0=m-1;
		for(j=0;j<m_ClmNum;j++)
		{	
			j0=j*nStep;
			if((j==(m_ClmNum-1))&&((j0>(n-1))||(j0<(n-1))))
				j0=n-1;
			x[k]=(i0)/1000.; 
			y[k]=(j0)/1000.; 
			g[k]=data[i0][j0];
			k=k+1;
		}
	}
////// To form metrix of factors  //a—factors matrix,g—anomaly, b—?///////////
	fact1(a,x,y,g,b,SampleNum,MaxOrder,m_OrderNumRow,m_OrderNumClm);

	svd_standalone(a,ItemNum,ItemNum,u,v,s,1);
/////// To resolve metrix equation ////////////
	for(i=0;i<ItemNum;i++)
	{	for(j=0;j<ItemNum;j++)
		{
			if(fabs(s[j])>1e-40)
				v[i][j]=v[i][j]/fabs(s[j]);
			else
				v[i][j]=0.0;
		}
	}
	tran(u,ItemNum);
	metrimult(v,u,ItemNum,ItemNum,ItemNum,a);

	for(i=0;i<ItemNum;i++)
	{	p=0.0;
		for(j=0;j<ItemNum;j++)
			p+=a[i][j]*b[j];
		s[i]=p;
	}
///////////////////////////////////////////
	for(i=0;i<m;i++)
	{	x0=i/1000.;
		for(j=0;j<n;j++)
		{	y0=j/1000.;
			tsv1(b,x0,y0,MaxOrder,m_OrderNumRow,m_OrderNumClm);
			p=0.0;
			for(k=0;k<ItemNum;k++)
				p+=s[k]*b[k];
			
			data[i][j] = p;
		}
	}

	delete[] a[0];
	delete[] a;
	delete[] u[0];
	delete[] u;
	delete[] v[0];
	delete[] v;
	delete[] x;
	delete[] y;
	delete[] g;
	delete[] b;
	delete[] s;
    return;
}
